#include <intrins.h>
#include <stdio.h>
#include "pid.h"



static float			Kp = 0.15;//0.075;// 1.9;//1.4//1.6;//[2.56 : most close value]
static float			Ki = 0;//0.003;//0.03;//0.0147;
static float			Kd = 0;//10.0;//0.1;


//static float			Kp = 1.8;// 1.9;//1.4//1.6;//[2.56 : most close value]
//static float			Ki = 0.025;//0.03;//0.0147;
//static float			Kd = 10;//10.0;//0.1;
static int				PID_samples[PID_SAMPLE_SIZE] = {0};
static int				PID_index = 0;
static int				PID_ep = 0;
static long				PID_sum = 0;	

//void PID_Set(float P, float I, float D)
//{
//	Kp = P;
//	Ki = I;
//	Kd = D;
//}

int PID(int e)
{
	int		f;

	PID_sum -= PID_samples[PID_index];
	PID_samples[PID_index++] = e;
	if(PID_index >= sizeof(PID_samples)/sizeof(int))
	{
		PID_index = 0;
	}
	PID_sum += e;
	
	f = Kp*e + Ki*PID_sum + Kd*(e - PID_ep);	
	
	
//	{
//		float P,I,D;
//		
//		P = Kp*e;
//		I = Ki*PID_sum;
//		D = Kd*(e - PID_ep);		
//		printf("%d\t%d\t%.2f\t%.2f\t%.2f\r\n", e, f, P, I, D);
//	}
	
	
	if(f > 100) f = 100;
	else if(f < 0) f = 0;

	PID_ep = e;
	
	return f;
}

//void PID_Reset()
//{
//	PID_index = 0;
//	PID_ep 		= 0;
//	PID_sum		= 0;
//}

